/**
  ******************************************************************************
  * @author     Chris
  * @since      2023/10/6 22:31
  *
  * @file       hd_mpu6050.c
  * @brief      Xxx Hardware Driver.
  *
  * @note       This file contains the hardware driver for the Xxx.
  *
  * @warning    None.
  ******************************************************************************
  * Change Logs:
  *   Date           Author       Notes
  *   2023/10/6     Chris        the first version
  *
  ******************************************************************************
  */

#include "hd_mpu6050.h"
#include "stm32g4xx_ll_bus.h"
#include "stm32g4xx_ll_gpio.h"
#include "stm32g4xx_ll_i2c.h"
#include "stm32g4xx_ll_rcc.h"
#include "pd_h_i2c.h"

/** region Declaration and Definition Region */

/** region Variables and Constants Region */

#define     MPU6050_ADDRESS         0xD0

#define     MPU6050_SMPLRT_DIV      0x19
#define     MPU6050_CONFIG          0x1A
#define     MPU6050_GYRO_CONFIG     0x1B
#define     MPU6050_ACCEL_CONFIG    0x1C

#define     MPU6050_ACCEL_XOUT_H    0x3B
#define     MPU6050_ACCEL_XOUT_L    0x3C
#define     MPU6050_ACCEL_YOUT_H    0x3D
#define     MPU6050_ACCEL_YOUT_L    0x3E
#define     MPU6050_ACCEL_ZOUT_H    0x3F
#define     MPU6050_ACCEL_ZOUT_L    0x40
#define     MPU6050_TEMP_OUT_H      0x41
#define     MPU6050_TEMP_OUT_L      0x42
#define     MPU6050_GYRO_XOUT_H     0x43
#define     MPU6050_GYRO_XOUT_L     0x44
#define     MPU6050_GYRO_YOUT_H     0x45
#define     MPU6050_GYRO_YOUT_L     0x46
#define     MPU6050_GYRO_ZOUT_H     0x47
#define     MPU6050_GYRO_ZOUT_L     0x48

#define     MPU6050_PWR_MGMT_1      0x6B
#define     MPU6050_PWR_MGMT_2      0x6C
#define     MPU6050_WHO_AM_I        0x75

/** endregion */

/** region Function Declaration Region */

void HD_MPU6050_WriteReg(uint8_t regAddress, uint8_t dat);

uint8_t HD_MPU6050_ReadReg(uint8_t regAddress);

/** endregion */

/** endregion */


/** region Public Definition Region */

void HD_MPU6050_Init(void) {
    // 硬件 I2C 初始化
    PD_H_I2C1_Init();

    // MPU6050 初始化
    HD_MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
    HD_MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
    HD_MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
    HD_MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
    HD_MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
    HD_MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}


uint8_t HD_MPU6050_GetID(void) {
    return HD_MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

void HD_MPU6050_GetData(HD_MPU6050_DataTypeDef *MPU6050_DataStruct) {
    // 三轴陀螺仪
    uint8_t dataH = HD_MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
    uint8_t dataL = HD_MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
    MPU6050_DataStruct->accX = (dataH << 8) | dataL;
    dataH = HD_MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
    MPU6050_DataStruct->accY = (dataH << 8) | dataL;
    dataH = HD_MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
    MPU6050_DataStruct->accZ = (dataH << 8) | dataL;

    // 温度
    dataH = HD_MPU6050_ReadReg(MPU6050_TEMP_OUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_TEMP_OUT_L);
    MPU6050_DataStruct->temp = (dataH << 8) | dataL;

    // 三轴加速度
    dataH = HD_MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
    MPU6050_DataStruct->gyroX = (dataH << 8) | dataL;

    dataH = HD_MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
    MPU6050_DataStruct->gyroY = (dataH << 8) | dataL;

    dataH = HD_MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
    dataL = HD_MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
    MPU6050_DataStruct->gyroZ = (dataH << 8) | dataL;
}

/** endregion */


/** region Private Definition Region */

void HD_MPU6050_WriteReg(uint8_t regAddress, uint8_t dat) {
    PD_H_I2C_WriteBytes(I2C1, MPU6050_ADDRESS, regAddress, &dat, 1);
}

uint8_t HD_MPU6050_ReadReg(uint8_t regAddress) {
    uint8_t buf[2];
    PD_H_I2C_ReadBytes(I2C1, MPU6050_ADDRESS, regAddress, buf, 1);
    return buf[0];
}

/** endregion */
